#include "antColonyOptimization.h"

antColonyOptimization::antColonyOptimization() {
//    vert = vertPara;
//    drawVert = vertPara;
    //tool::transposeMatrix(vert);

}

void antColonyOptimization::G2D(int k) {

    antsAdjacencyMatrix[k].clear();
    antsAdjacencyMatrix[k] = std::vector<std::vector<bool>>(citys, std::vector<bool>(citys, false));

    for (int i = 0; i < citys; i++) {
        for (int j = 0; j < citys; j++) {
            if (judgeAdjacency(i, j))
                antsAdjacencyMatrix[k][i][j] = true;    //true为通达
        }
    }

//    for (int i = 0; i < k; i++) {
//        for (int j = 0; j < k; j++) {
//            std::cout << adjacencyMatrix[i][j] << " ";
//        }
//        std::cout << std::endl;
//    }

    return;
}

bool antColonyOptimization::judgeAdjacency(int p1, int p2) {
    if (p1 == p2)
        return false;

    int x1, x2, y1, y2;

    x1 = p1 / rows;
    x2 = p2 / rows;
    y1 = p1 % rows;
    y2 = p2 % rows;

    if (fabs(x1 - x2) <= 1 && fabs(y1 - y2) <= 1 && vert[x2][y2] == 0) {
        return true;
    }

    return false;
}

void antColonyOptimization::init() {

    rows = vert.size();
//    start.first = 0;
//    start.second = 0;
//    stop.first = vert.size()-1;
//    stop.second = vert.size()-1;

    rho = 0.3;//evaporation parameter，挥发参数，每次信息素要挥发的量 信息素挥发因子ρ
    alpha = 1;
    beta = 2;// alpha 和 beta分别表示pheromone(信息素)和heuristic的比重
    citys = rows * rows;
    antNum = citys;
//    for (int i = 0; i < rows; i++) {
//        for (int j = 0; i < rows; j++) {
//            if (!vert[i][j]) {
//                ++antNum;
//            }
//        }
//    }
    pathes = std::vector<std::vector<pair_int>>(antNum, std::vector<pair_int>());
    pheromone = std::vector<std::vector<double>>(citys, std::vector<double>(citys));
    heuristic = std::vector<std::vector<double>>(rows, std::vector<double>(rows));
    //信息素矩阵
    tau = std::vector<std::vector<double>>(citys, std::vector<double>(citys, 0));

    ants = std::vector<ant>(antNum, ant(start));
    antsAdjacencyMatrix = std::vector<towDims>(antNum, std::vector<std::vector<bool>>(citys, std::vector<bool>(citys, false)));
    for(int i = 0; i < antNum; ++i) {
        G2D(i);
    }

    for (int i = 0; i < citys; i++) {
        for (int j = 0; j < citys; j++) {
            if (judgeAdjacency(i, j))
                tau[i][j] = 0.05;//初始化信息素
        }
    }

    for (int i = 0; i < rows; i++) {
        for (int j = 0; j < rows; j++) {
            if (!vert[i][j]) {
                heuristic[j][i] = 1.0 / (tool::distance(std::make_pair(j, i), stop) + MINIMUM);
            }
        }
    }
    //G2D();

    return;
}

pair_int antColonyOptimization::selectNext(pair_int k, int n) {


    double sum = 0.0, sums = 0.0, t;
    std::vector<pair_di> temp;//first为T^A*H^B(pheromone ^ alpha * herustic ^ beta),second为城市序号
    int t0 = tool::pairToNum(rows, k);
    for(int i = (t0 - rows -1)>0 ? t0-rows-1:0; i < citys; ++i){
        if(tool::numToPair(rows, i) == k)
            continue;
        else if(i > t0 + rows + 1)
            break;
        else if(antsAdjacencyMatrix[n][t0][i]){

            if(!judgeGo(t0, i))
                continue;
            t = pow(tau[t0][i], alpha) * pow(heuristic[k.first][k.second], beta);

            //std::cout << "tau[t0][i]:" << tau[t0][i] << std::endl;
            //std::cout << "heuristic[k.first][k.second]:" << heuristic[k.first][k.second] << std::endl;
            sum += t;
            temp.push_back(std::make_pair(t, i));
        }
    }

    double rnd = (double)(rand()) / (double)RAND_MAX;//产生0..1的随机数
    int num = temp.size();
    if(!num)
        return std::make_pair(-1, -1);

    rnd *= sum;

    for(int i=0; i<num; ++i){
        sums += temp[i].first;

        if(sums >= rnd){
            pair_int t1 = tool::numToPair(rows, temp[i].second);
            ++ants[n].num;
            return t1;
        }
    }

    return std::make_pair(-1, -1);
}

//更新禁忌矩阵
void antColonyOptimization::updateD(std::vector<pair_int> passBy, int n) {
    int t = tool::pairToNum(rows, ants[n].current);
    int k = passBy.size();

    for(int i = 0; i < k; ++i) {
        int t1 = tool::pairToNum(rows, passBy[i]);
        antsAdjacencyMatrix[n][t][t1] = false;//false为禁忌
    }

    return;
}

void inline antColonyOptimization::updatePheromone() {

    int n1, n2;

    for (int i = 0; i < rows; i++) {
        for (int j = 0; j < rows; j++) {
            if (tau[i][j]) {
                tau[i][j] *= (1 - rho);
            }
        }
    }

    for(int i = 0; i < antNum; ++i) {
        if(ants[i].last == ants[i].current){
            continue;
        }
        n1 = tool::pairToNum(rows, ants[i].last);
        n2 = tool::pairToNum(rows, ants[i].current);
        tau[n1][n2] += deltaTau;

        if(ants[i].current == start) {
            ants[i].clear(start);
        }

    }
}


//判断斜路是否可通行
bool antColonyOptimization::judgeGo(int cur, int next) {
    pair_int sta = tool::numToPair(rows, cur);
    pair_int end = tool::numToPair(rows, next);

    Direction direct = tool::judgeDirection(sta, end);
    if(direct == LOWERRIGHT) {
        if(vert[sta.second+1][sta.first] && vert[sta.second][sta.first+1]) {
            return false;
        }
    } else if(direct == LOWERLEFT) {
        if(vert[sta.second][sta.first-1] && vert[sta.second+1][sta.first]) {
            return false;
        }
    } else if(direct == UPPERLEFT) {
        if(vert[sta.second][sta.first-1] && vert[sta.second-1][sta.first]) {
            return false;
        }
    } else if(direct == UPPERRIGHT) {
        if(vert[sta.second][sta.first+1] && vert[sta.second-1][sta.first]) {
            return false;
        }
    }

    return true;
}

std::vector<QPoint> antColonyOptimization::process() {


    best.clear();
    ants.clear();
    pheromone.clear();
    heuristic.clear();
    tau.clear();
    antsAdjacencyMatrix.clear();
    pathes.clear();
    bestPathes.clear();


    srand((int)time(0));
    init();

    min = scale*citys;         //设置为较大
    for(int i=0; i<400; ++i){

        for(int j = 0; j < antNum; ++j) {
            if(ants[j].status == LEAVE) {
                ants[j].path.push_back(ants[j].current);
                ants[j].current = selectNext(ants[j].current, j);
                if (ants[j].current == std::make_pair(-1, -1)) {
                    ants[j].clear(start);
                    G2D(j);
                    continue;
                }
                ants[j].dis += tool::distance(ants[j].last, ants[j].current);
                updateD(ants[j].path, j);
                ants[j].last = ants[j].current;

                if(ants[j].current == stop) {
                    G2D(j);
                    ants[j].path.push_back(ants[j].current);
                    ants[j].status = RETURN;
                    if(ants[j].dis < min) {
                        min = ants[j].dis;
                        best = ants[j].path;
                        std::cout << "min:" << min << std::endl;
                    }
                }
            } else {
                ants[j].current = ants[j].path[--ants[j].num];
                ants[j].last = ants[j].current;

                if(ants[j].current == start) {
                    ants[j].clear(start);
                    //continue;
                }

            }
        }
        updatePheromone();
    }

    //tool::optimizationRoute(best);

    qtDraw = tool::routeToPoint(best, start, scale);
//    for(int i=0;i<best.size();i++) {
//        printf("(%d,%d) ",best[i].first, best[i].second);
//    }
//    std::cout << std::endl;
    //d.show(best);

    //cv::waitKey(0);
    //std::cout << std::endl;

    return qtDraw;
}
